#if !defined(MAP_FILTER_IMPL_H_)
#define MAP_FILTER_IMPL_H_

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <limits>
#include <list>
#include <memory>
#include <mutex>
#include <set>

#include "common/map_info.h"
#include "map_filter.h"
// #include "perception_visualizer_debug.h"
namespace autodrive {
namespace perception {
class MapFilterImpl final {
 public:
  MapFilterImpl();
  MapFilterImpl(MapFilterImpl &&) = delete;
  MapFilterImpl(const MapFilterImpl &) = delete;
  MapFilterImpl &operator=(MapFilterImpl &&) = delete;
  MapFilterImpl &operator=(const MapFilterImpl &) = delete;
  ~MapFilterImpl();

 public:
  /// @brief point distance item
  struct PointDistanceItem {
    /// @brief source point index
    std::uint32_t src_index;
    /// @brief desitination point index
    std::uint32_t dst_index;
    /// @brief distance
    float distance;
  };

  /// @brief overlap domain
  struct OverlapDomain {
    /// @brief start index
    std::uint32_t start_index;
    /// @brief stop index
    std::uint32_t stop_index;
  };

  /// @brief match item
  struct MatchItem {
    /// @brief predict map element point set pointer
    MapPointsElementPtr predict_map_points_element_ptr;
    /// @brief current map element point set pointer
    MapPointsElementConstPtr current_map_points_element_ptr;
    /// @brief current map element point to predict map element point min
    /// distance list
    std::vector<PointDistanceItem> predict_to_current_min_dist_list;
    /// distance list
    std::vector<PointDistanceItem> current_to_predict_min_dist_list;
    /// @brief match score
    float score;
    /// @brief predict map points element index
    std::int32_t predict_index;
    /// @brief current map points element index;
    std::int32_t current_index;
  };

  /// @brief chamfer distance result
  struct ChamferDistanceResult {
    /// @brief p1 to p2 min distance point index list
    std::vector<PointDistanceItem> p1_p2_min_dist_list;
    /// @brief p2 to p1 min distance point index list
    std::vector<PointDistanceItem> p2_p1_min_dist_list;
    /// @brief distance
    float distance;
  };

 public:
  /// @brief init
  /// @param para map filter parameter
  /// @throw
  void Init(MapFilter::MapFilterParameter const &para) noexcept(false);

  /// @brief is init flag
  /// @return init flag
  bool IsInit() noexcept { return is_init_; }

  /// @brief do map filter
  /// @param map map data
  /// @param pose current pose
  /// @throw std::runtime_error uninitialized
  /// @throw std::runtime_error predict road edge error
  /// @throw std::runtime_error predict lane divider error
  /// @throw std::runtime_error predict center line error
  void operator()(Map &map, Eigen::Affine3d const &pose) noexcept(false);

 protected:
  /// @brief show map info information
  void show_info() noexcept;

  /// @brief predict point with source point , history pose and current pose
  /// @param src source point
  /// @param history_pose history pose
  /// @param current_pose current pose
  /// @return predicted point
  /// @throw std::runtime_error current pose is inversable
  Eigen::Vector3d predict(Eigen::Vector3d const &src,
                          Eigen::Affine3d const &history_pose,
                          Eigen::Affine3d const &current_pose) noexcept(false);

  /// @brief get overlap domain
  /// @param p1_to_p2_min_dist_list pointset1 to pointset2 minimal distance list
  /// @param p2_to_p1_min_dist_list pointset2 to pointset1 minimal distance list
  /// @return <p1, p2> overlap domain
  std::pair<OverlapDomain, OverlapDomain> get_overlap_domain(
      std::vector<PointDistanceItem> const &p1_to_p2_min_dist_list,
      std::vector<PointDistanceItem> const &p2_to_p1_min_dist_list) noexcept;

  /// @brief predict map element
  /// @tparam T map element type
  /// @param history_map_element_set history map element set
  /// @param parameter filter parameter
  /// @param history_pose history pose
  /// @param current_pose current pose
  /// @param predict_map_element_set predict map element set
  template <typename T>
  void predict_map_element(
      MapElementSet<T> const &history_map_element_set,
      MapFilter::MapFilterParameter const &parameter,
      Eigen::Affine3d const &history_pose, Eigen::Affine3d const &current_pose,
      MapElementSet<T> &predict_map_element_set) noexcept(false) {
    auto predictMapPointsElement{
        [this, &parameter](MapElementPointSet const &pointsSetSrc,
                           Eigen::Affine3d const &historyPose,
                           Eigen::Affine3d const &currentPose,
                           MapElementPointSet &pointsSetDst) {
          auto const &pointNumber{pointsSetSrc.size()};
          auto const &roi{parameter.roi};
          pointsSetDst.clear();
          for (std::uint32_t i{0}; i < pointNumber; ++i) {
            auto p{pointsSetSrc.at(i)};
            auto const &predictPoint(
                predict({p.x, p.y, p.z}, historyPose, currentPose));
            auto const &x{clip<float>(predictPoint.x())};
            auto const &y{clip<float>(predictPoint.y())};
            auto const &z{clip<float>(predictPoint.z())};

            if (is_point_in_roi({x, y}, roi)) {
              if (!pointsSetDst.full()) {
                pointsSetDst.add({x, y, z});
              }
            }
          }
        }};

    constexpr std::int32_t minimalPoints{5};
    auto const &historyMapElementNumber{history_map_element_set.size()};
    predict_map_element_set.clear();
    for (std::uint32_t i{0}; i < historyMapElementNumber; ++i) {
      auto const &historyMapElement{history_map_element_set.at(i)};
      if (predict_map_element_set.full()) {
        break;
      }
      auto predictMapElement{predict_map_element_set.end()};
      try {
        predictMapPointsElement(historyMapElement.points, history_pose,
                                current_pose, predictMapElement->points);

        if (predictMapElement->points.size() >= minimalPoints) {
          predictMapElement->life_time = historyMapElement.life_time;
          predict_map_element_set.resize(predict_map_element_set.size() + 1);
        }
      } catch (const std::exception &e) {
        throw std::runtime_error("predict map element point error, " +
                                 std::string(e.what()));
      }
    }
  }

  /// @brief match map element
  /// @tparam T map element type
  /// @param history_map_element_set predict map element set
  /// @param current_map_element_set current map element set
  /// @param parameter match parameter
  /// @param matched_list matched list
  /// @param unmatched_list unmatched list
  template <typename T>
  std::pair<bool, MatchItem> match_map_element(
      MapElementSet<T> &predict_map_element_set,
      MapElementSet<T> const &current_map_element_set,
      MapFilter::MatchParameter const &parameter) noexcept {
    auto const &predictMapElementNumber{predict_map_element_set.size()};
    for (std::uint32_t i{0}; i < predictMapElementNumber; ++i) {
      MatchItem matchItem{.predict_map_points_element_ptr{nullptr},
                          .current_map_points_element_ptr{nullptr},
                          .score{std::numeric_limits<float>::max()}};
      auto &predictMapElement{predict_map_element_set.at(i)};

      bool isMatched{false};

      auto const &currentMapElementNumber{current_map_element_set.size()};
      for (std::uint32_t j{0}; j < currentMapElementNumber; ++j) {
        auto &currentMapElement{current_map_element_set.at(j)};

        auto const &currentPointNumber{currentMapElement.points.size()};
        if (currentPointNumber <= 0) {
          continue;
        }

        ChamferDistanceResult chamferDistanceResult;
        calculate_chamfer_distance(predictMapElement.points,
                                   currentMapElement.points, parameter.roi,
                                   chamferDistanceResult);

        // std::cout << "---->matchItem(history, current, score)"
        //           << predictMapElement.points.size() << ","
        //           << currentMapElement.points.size() << ","
        //           << chamferDistanceResult.distance << std::endl;

        if (chamferDistanceResult.distance < matchItem.score) {
          matchItem.predict_map_points_element_ptr = &predictMapElement;
          matchItem.current_map_points_element_ptr = &currentMapElement;
          matchItem.predict_to_current_min_dist_list =
              chamferDistanceResult.p1_p2_min_dist_list;
          matchItem.current_to_predict_min_dist_list =
              chamferDistanceResult.p2_p1_min_dist_list;
          matchItem.score = chamferDistanceResult.distance;
          matchItem.current_index = j;
          matchItem.predict_index = i;
          isMatched = true;
        }
      }

      bool isCurrentOverlapMatch{false};
      bool isCurrentOutlierMatch{true};

      auto const &maxChamferScore{19998};
      if (isMatched && matchItem.score < maxChamferScore) {
        // std::cout << "+++++++++++++++++++>matchItem(current, predict, score)"
        //           << matchItem.current_map_points_element_ptr->points.size()
        //           << ","
        //           << matchItem.predict_map_points_element_ptr->points.size()
        //           << "," << matchItem.score << std::endl;
        auto const &overlapDomainPair{
            get_overlap_domain(matchItem.predict_to_current_min_dist_list,
                               matchItem.current_to_predict_min_dist_list)};

        auto const &predictOverlapDomain{overlapDomainPair.first};
        auto const &currentOverlapDomain{overlapDomainPair.second};

        // bool isPredictMatch{false};
        // float predictMatchPoints{0};

        float minPoints{0};

        float minDistThreshold{parameter.min_distance_threshold};
        float minDistRatioThreshold{0.3};

        float maxPoints{0};
        float maxDistThreshold{parameter.max_distance_threshold};
        float maxDistRatioThreshold{0.8};

        if (currentOverlapDomain.stop_index >
            currentOverlapDomain.start_index) {
          for (std::uint32_t i{currentOverlapDomain.start_index};
               i < currentOverlapDomain.stop_index + 1; i++) {
            auto const &distItem{
                matchItem.current_to_predict_min_dist_list.at(i)};
            if (is_greater(minDistThreshold, distItem.distance)) {
              ++minPoints;
            }
            if (is_greater(distItem.distance, maxDistThreshold)) {
              ++maxPoints;
            }
          }

          if (matchItem.current_map_points_element_ptr != nullptr) {
            auto const &currentPointNumber{
                matchItem.current_map_points_element_ptr->points.size()};
            auto const &predPointNumber{
                matchItem.predict_map_points_element_ptr->points.size()};
            if (currentPointNumber > 0) {
              auto const &minDistRatio{minPoints /
                                       static_cast<float>(currentPointNumber)};
              if (is_greater(minDistRatio, minDistRatioThreshold)) {
                // {
                //   auto &perceptionVisualizerDebug{
                //       autodrive::perception::PerceptionVisualizerDebug::
                //           GetInstance()};
                //   perceptionVisualizerDebug.UpdateMatch(
                //       matchItem.predict_map_points_element_ptr->points,
                //       matchItem.current_map_points_element_ptr->points,
                //       "ChamferDistance: " + std::to_string(matchItem.score) +
                //           ", current num: " +
                //           std::to_string(currentMapElementNumber) +
                //           ", matched 1");
                // }
                // isCurrentOverlapMatch = true;
                return std::make_pair(true, matchItem);
              }
            }
            if (predPointNumber > 0) {
              auto const &minDistRatio{minPoints /
                                       static_cast<float>(predPointNumber)};
              if (is_greater(minDistRatio, minDistRatioThreshold)) {
                // {
                //   auto &perceptionVisualizerDebug{
                //       autodrive::perception::PerceptionVisualizerDebug::
                //           GetInstance()};
                //   perceptionVisualizerDebug.UpdateMatch(
                //       matchItem.predict_map_points_element_ptr->points,
                //       matchItem.current_map_points_element_ptr->points,
                //       "ChamferDistance: " + std::to_string(matchItem.score) +
                //           ", current num: " +
                //           std::to_string(currentMapElementNumber) +
                //           ", matched 2");
                // }
                // isCurrentOverlapMatch = true;
                return std::make_pair(true, matchItem);
              }
            }
          }

          if (maxPoints > 0) {
            auto const &maxDistRaio{
                maxPoints /
                static_cast<float>(currentOverlapDomain.stop_index -
                                   currentOverlapDomain.start_index + 1)};
            if (is_greater(maxDistRaio, maxDistRatioThreshold)) {
              isCurrentOutlierMatch = false;
            } else {
              isCurrentOutlierMatch = true;
            }
          }
          if (isCurrentOutlierMatch) {
            if (is_greater(parameter.chamfer_distance_threshold,
                           matchItem.score) ||
                isCurrentOverlapMatch) {
              // {
              //   auto &perceptionVisualizerDebug{
              //       autodrive::perception::PerceptionVisualizerDebug::
              //           GetInstance()};
              //   perceptionVisualizerDebug.UpdateMatch(
              //       matchItem.predict_map_points_element_ptr->points,
              //       matchItem.current_map_points_element_ptr->points,
              //       "ChamferDistance: " + std::to_string(matchItem.score) +
              //           ", current num: " +
              //           std::to_string(currentMapElementNumber) +
              //           ", matched 3");
              // }
              return std::make_pair(true, matchItem);
            }
          }
        }
      }
      // {
      //   if (matchItem.predict_map_points_element_ptr != nullptr &&
      //       matchItem.current_map_points_element_ptr != nullptr) {
      //     auto &perceptionVisualizerDebug{
      //         autodrive::perception::PerceptionVisualizerDebug::GetInstance()};
      //     perceptionVisualizerDebug.UpdateMatch(
      //         matchItem.predict_map_points_element_ptr->points,
      //         matchItem.current_map_points_element_ptr->points,
      //         "ChamferDistance: " + std::to_string(matchItem.score) +
      //             ", current num: " + std::to_string(currentMapElementNumber)
      //             +
      //             ", matched 00000");
      //   }
      // }
    }

    return std::make_pair(false, MatchItem());
  }

  /// @brief remove repeat map element
  /// @tparam T
  /// @param parameter
  /// @param src_map_element_set
  /// @param dst_map_element_set
  template <typename T>
  void remove_repeat_map_element(
      MapFilter::MapFilterParameter const &parameter,
      MapElementSet<T> &src_map_element_set,
      MapElementSet<T> &dst_map_element_set) noexcept(false) {
    dst_map_element_set.clear();
    auto const &srcMapEleNum{src_map_element_set.size()};

    for (std::int32_t i = 0; i < srcMapEleNum; ++i) {
      auto const &pointNum{src_map_element_set.at(i).points.size()};
      if (pointNum < 1) {
        continue;
      }
      dst_map_element_set.add(src_map_element_set.at(i));
      src_map_element_set.at(i).points.clear();

      while (true) {
        bool hasMatchedItem{false};
        auto &&matchPair{match_map_element(dst_map_element_set,
                                           src_map_element_set,
                                           parameter.match_paramemter)};
        hasMatchedItem = matchPair.first;
        if (!hasMatchedItem) {
          break;
        }

        auto &matchItem{matchPair.second};
        fusion_match_points(parameter.fusion_parameter, matchItem);

        auto &srcMatched{src_map_element_set.at(matchItem.current_index)};
        srcMatched.points.clear();

        matchItem.predict_map_points_element_ptr->life_time =
            std::max(matchItem.predict_map_points_element_ptr->life_time,
                     matchItem.current_map_points_element_ptr->life_time);
      }
    }
  }

  /// @brief fusion match item and unmatch item
  /// @tparam T map element type
  /// @param predict_map_element_set predict map element set
  /// @param matched_list match list
  /// @param unmatched_list unmatch list
  /// @param parameter fusion parameter
  /// @param history_map_element_set history map element set
  /// @throw std::runtime_error historyFusionStartIndex larger than
  /// historyFusionStopIndex
  /// @throw std::runtime_error currentFusionStartIndex larger than
  /// currentFusionStopIndex
  /// @throw std::runtime_error histMergeLength is not equal to
  /// currentMergeLength
  template <typename T>
  void fusion_map_elment(
      MapFilter::MapFilterParameter const &parameter,
      MapElementSet<T> &predict_map_element_set,
      MapElementSet<T> &current_map_element_set,
      MapElementSet<T> &history_map_element_set) noexcept(false) {
    history_map_element_set.clear();

    while (true) {
      bool hasMatchedItem{false};
      auto &&matchPair{match_map_element(predict_map_element_set,
                                         current_map_element_set,
                                         parameter.match_paramemter)};
      hasMatchedItem = matchPair.first;
      if (!hasMatchedItem) {
        break;
      }

      auto &matchItem{matchPair.second};
      auto para{parameter.fusion_parameter};
      para.x_low_limit = -5;
      fusion_match_points(parameter.fusion_parameter, matchItem);

      auto &currentMatched{current_map_element_set.at(matchItem.current_index)};
      currentMatched.points.clear();

      constexpr std::int32_t lifeTimeOffset{2};
      constexpr std::int32_t lifeTimeOffsetBonus{lifeTimeOffset * 10000};
      std::int64_t lifeTime{
          matchItem.predict_map_points_element_ptr->life_time};
      if (lifeTime > parameter.lifetime_bonus_threshold) {
        matchItem.predict_map_points_element_ptr->life_time =
            clip<std::int32_t>(lifeTime + lifeTimeOffsetBonus);
      } else {
        matchItem.predict_map_points_element_ptr->life_time =
            clip<std::int32_t>(lifeTime + lifeTimeOffset);
      }
    }

    // add unmatched current into history
    auto const &currentMapElementNumber{current_map_element_set.size()};
    for (std::uint32_t i{0}; i < currentMapElementNumber; ++i) {
      auto &currentMapElement{current_map_element_set.at(i)};
      auto const &currentPointNumber{currentMapElement.points.size()};
      if (currentPointNumber <= 0) {
        continue;
      }

      if (history_map_element_set.full()) {
        break;
      }

      auto mapElement{history_map_element_set.end()};
      mapElement->life_time = 0;
      mapElement->points.clear();

      for (std::uint32_t i{0}; i < currentMapElement.points.size(); ++i) {
        auto const &p{currentMapElement.points.at(i)};
        if (is_greater(p.x, parameter.fusion_parameter.x_low_limit)) {
          mapElement->points.add(p);
        }
      }
      if (mapElement->points.size() > 0) {
        history_map_element_set.resize(history_map_element_set.size() + 1);
      }
    }

    // use predict to update history
    constexpr std::int32_t lifeTimeOffset{-1};
    for (std::uint32_t i{0}; i < predict_map_element_set.size(); ++i) {
      if (history_map_element_set.full()) {
        break;
      }

      auto historyMapElement{history_map_element_set.end()};
      auto const &predictMapElement{predict_map_element_set.at(i)};

      historyMapElement->points = predictMapElement.points;
      auto const &p{
          predictMapElement.points.at(predictMapElement.points.size() - 1)};
      std::int64_t lifeTime{predictMapElement.life_time};
      if (is_greater(p.x, parameter_.fusion_parameter.x_low_limit)) {
        lifeTime = lifeTime + lifeTimeOffset;
      }
      historyMapElement->life_time = clip<std::int32_t>(lifeTime);
      history_map_element_set.resize(history_map_element_set.size() + 1);
    }
  }

  /// @brief fusion point with weight,weight*p1+(1-weight)*p2
  /// @param p1 first point
  /// @param p2 second point
  /// @param weight weight
  /// @return fusioned point
  MapElementPoint fusion_point(MapElementPoint const &p1,
                               MapElementPoint const &p2,
                               float weight) noexcept;

  void fusion_match_points(MapFilter::FusionParamter const &parameter,
                           MatchItem &matchItem) noexcept(false);

  /// @brief judge d1>d2?
  /// @tparam T type, float or double
  /// @param d1 data 1
  /// @param d2 data 2
  /// @return d1>d2 return true, otherwise false
  template <typename T>
  bool is_greater(T const &d1, T const &d2) noexcept {
    static_assert(std::is_floating_point_v<T>,
                  "data type must be float or double");
    bool cmp1{std::abs(d1 - d2) > std::numeric_limits<T>::epsilon()};
    bool cmp2{d1 > d2};
    return cmp1 && cmp2;
  }
  /// @brief clip data in range
  /// @tparam T output data type
  /// @param data data to clip
  /// @return clipped data
  template <typename T>
  T clip(double data) {
    static_assert(std::is_arithmetic_v<T>, "clip type is not arithmetic type");
    bool isNegative{false};
    if (std::is_floating_point_v<T> && data < 0) {
      data = -data;
      isNegative = true;
    }

    auto value{std::clamp<double>(data, std::numeric_limits<T>::min(),
                                  std::numeric_limits<T>::max())};
    if (isNegative) {
      value = -value;
    }

    return static_cast<T>(value);
  }

  /// @brief calcuate point distance
  /// @tparam T point data type
  /// @param p1 first point
  /// @param p2 second point
  /// @return point distance
  template <typename T>
  float calculate_point_distance(Point3D<T> const &p1,
                                 Point3D<T> const &p2) noexcept {
    static_assert(std::is_arithmetic_v<T>,
                  "calculate_point_distance type is not arithmetic type");
    double &&value{(p2.x - p1.x) * (p2.x - p1.x) +
                   (p2.y - p1.y) * (p2.y - p1.y)};
    value = std::sqrt(value);
    return clip<float>(value);
  }

  /// @brief calcuate chamfer distance
  /// @param template_points template points
  /// @param points_1 first point set
  /// @param points_2 second point set
  /// @param ROI roi
  /// @return chamfer distance result
  void calculate_chamfer_distance(MapElementPointSet const &points_1,
                                  MapElementPointSet const &points_2,
                                  MapFilter::ROI const &roi,
                                  ChamferDistanceResult &result) noexcept;

  /// @brief judge if point in roi
  /// @param p point
  /// @param roi roi
  /// @return if in roi ture, else false
  bool is_point_in_roi(Point2f const &p, MapFilter::ROI const &roi) noexcept;

 private:
  /// @brief init flag
  bool is_init_;
  /// @brief  init call once flag
  mutable std::once_flag init_flag_;
  /// @brief map filter paramters
  MapFilter::MapFilterParameter parameter_;
  /// @brief history map pointer
  std::shared_ptr<Map> history_map_ptr_;
  /// @brief history map tmp pointer
  std::shared_ptr<Map> history_map_tmp_ptr_;
  /// @brief history pose
  Eigen::Affine3d history_pose_;
  /// @brief fusion result pointer
  std::shared_ptr<MapElementPointSet> fusion_result_ptr_;

  /// @brief road edge predict pointer
  std::shared_ptr<MapElementSet<RoadEdge>> road_edge_predict_ptr_;

  /// @brief road edge predict pointer
  std::shared_ptr<MapElementSet<LaneDivider>> lane_divier_predict_ptr_;

  /// @brief center line predict pointer
  std::shared_ptr<MapElementSet<CenterLine>> center_line_predict_ptr_;
};
}  // namespace perception
}  // namespace autodrive
#endif  // MAP_FILTER_IMPL_H_
